#include <Communication.h>

Comlcm::Comlcm(const std::string topic, const std::string ips)
{
    valid_topic = topic;
    valid_ip = ips;
}

bool Comlcm::Publish(const std::vector<CorLag> Lagoris)
{
    float px = 1500, py = 3155;
    // px = 
    lcm::LCM lcm(valid_ip);
    if (!lcm.good())
    {
        std::cout << "lcm 启动失败，请检查！！！" << std::endl;
        return 1;
    }

    for (size_t i = 0; i < Lagoris.size(); i++)
    {
        exlcm::coordinate_LAG send_data;
        send_data.color = Lagoris[i].Lcolor;
        send_data.center[0] = Lagoris[i].center.x() + px;
        send_data.center[1] = Lagoris[i].center.y() + py;
        send_data.center[2] = 0.0;
        send_data.id = Lagoris[i].id;
        send_data.angle = 0.;
        send_data.orientation = 0.;
        send_data.normal[0] = 0.0;
        send_data.normal[1] = 0.0;
        send_data.normal[2] = 0.0;
        
        lcm.publish(valid_topic, &send_data);
    }
    
    return true;
}
